from enum import Enum
from queue import PriorityQueue
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import random


def r_ints(p1, p2):
    # return the points between the given two points
    I = [i - j for i, j in zip(p2, p1)]
    d = np.sqrt(np.sum(np.square(I)))
    I = [i / d for i in I]  # unit vector

    p_set = [p1]
    dis = np.sqrt(np.sum(np.square([i - j for i, j in zip(p1, p2)])))
    s = 1
    while dis >= (1.414 + 0.1):
        p_m = [round(i + j * s) for i, j in zip(p1, I)]
        dis = np.sqrt(np.sum(np.square([i - j for i, j in zip(p2, p_m)])))
        s = s + 1
        if p_m not in p_set:
            p_set.append(p_m)

    if p2 not in p_set:
        p_set.append(p2)

    return p_set


class MotionPlaner:
    def __init__(self, data, single_obstacle, start, goal, boundarys):
        """
        ****data descripe the buildings or obstacles****
        data: [z, a_x,a_y, b_x,b_y, c_x,c_y, d_x,d_y, h]
            z is botttom of the building in z-anix
            a,b,c,d are vertices of rectangle， a = (a_x, a_y)
            h is the height
        """
        # self.start = data[0]
        # self.goal = data[1]
        # super(MotionPlaner, self).__init__(start, goal)
        x_size = boundarys[0]
        y_size = boundarys[1]
        z_size = boundarys[2]
        # print("data.shape", data[1])
        # for i in range(1):
        #     if x_size <= max(data[i][0][1], data[i][0][3], data[i][0][5], data[i][0][7]):
        #         x_size = max(data[i][0][1], data[i][0][3], data[i][0][5], data[i][0][7])
        #     if y_size <= max(data[i][0][2], data[i][0][4], data[i][0][6], data[i][0][8]):
        #        y_size = max(data[i][0][2], data[i][0][4], data[i][0][6], data[i][0][8])+1
        #     if z_size <= data[i][0][-1]:
        #         z_size = data[i][0][-1]+1  # 根据输入的起点终点和障碍物的最大xyz值建模

        self.voxmap = create_voxmap(data[0], single_obstacle, x_size, y_size, z_size)  # voxmap及offset属于全局变量


    def plot3DModel(self, data_wall, data_pipe, start, goal, goal1, false_ceiling):
        # print("goal1", goal1)
        traj_goal = np.zeros(self.voxmap.shape, dtype=np.bool)
        # print("t", traj_goal)
        traj_goal[goal[0]][goal[1]][goal[2]] = True
        traj_start = np.zeros(self.voxmap.shape, dtype=np.bool)
        traj_start[start[0]][start[1]][start[2]] = True

        traj = np.zeros(self.voxmap.shape, dtype=np.bool)  # 管道路径初始化

        traj_1 = np.zeros(self.voxmap.shape, dtype=np.bool)  # 不可穿障碍物初始化
        traj_2 = np.zeros(self.voxmap.shape, dtype=np.bool)  # 可穿障碍物初始化
        traj_3 = np.zeros(self.voxmap.shape, dtype=np.bool)
        traj_4 = np.zeros(self.voxmap.shape, dtype=np.bool)
        traj_Nonconcrete = np.zeros(self.voxmap.shape, dtype=np.bool)  # 非结构墙

        data2 = data_wall[0]  # 不可穿障碍物
        # 修改场景，将非结构墙变成梁，即高度变小
        # data1 = data_wall[1]  # 可穿的墙

        # data3 = data_pipe[0]

        # data4 = [[10, 2, 20, 3, 20, 2, 27, 3, 27, 30]]

        # data4 = [[10, 2, 20, 3, 20, 2, 27, 3, 27, 30], [10, 4, 20, 5, 20, 4, 27, 5, 27, 30],
        #          [10, 2, 20, 5, 20, 2, 18, 5, 18, 30], [10, 2, 26, 5, 26, 2, 28, 5, 28, 30]]

        # data1 = np.array(data1)
        # for i in range(data1.shape[0]):
        #     z, a_x, a_y, b_x, b_y, c_x, c_y, d_x, d_y, h = data1[i, :]
        #     a = [a_x, a_y]
        #     b = [b_x, b_y]
        #     c = [c_x, c_y]
        #     d = [d_x, d_y]
        #     x_index, y_index = map_color_seg(a, b, c, d)
        #     traj_Nonconcrete[x_index.astype(int), y_index.astype(int), z:(z + h)] = True
        #     traj_2[x_index.astype(int), y_index.astype(int), z:(z + h)] = True

        data2 = np.array(data2)
        for i in range(data2.shape[0]):
            z, a_x, a_y, b_x, b_y, c_x, c_y, d_x, d_y, h = data2[i, :]
            a = [a_x, a_y]
            b = [b_x, b_y]
            c = [c_x, c_y]
            d = [d_x, d_y]
            x_index, y_index = map_color_seg(a, b, c, d)
            traj_1[x_index.astype(int), y_index.astype(int), z:(z + h)] = True

        # data3 = np.array(data3)
        # for i in range(data3.shape[0]):
        #     z, a_x, a_y, b_x, b_y, c_x, c_y, d_x, d_y, h = data3[i, :]
        #     a = [a_x, a_y]
        #     b = [b_x, b_y]
        #     c = [c_x, c_y]
        #     d = [d_x, d_y]
        #     x_index, y_index = map_color_seg(a, b, c, d)
        #     traj_3[x_index.astype(int), y_index.astype(int), z:(z + h)] = True
        #
        # data4 = np.array(data4)
        # for i in range(data4.shape[0]):
        #     z, a_x, a_y, b_x, b_y, c_x, c_y, d_x, d_y, h = data4[i, :]
        #     a = [a_x, a_y]
        #     b = [b_x, b_y]
        #     c = [c_x, c_y]
        #     d = [d_x, d_y]
        #     x_index, y_index = map_color_seg(a, b, c, d)
        #     traj_4[x_index.astype(int), y_index.astype(int), z:(z + h)] = True

        # 修改直接连接
        mylist = []
        out = []
        if len(goal1) != 0:
            # print("有节点")
            for i in range(len(goal1)):
                if np.all(tuple(start) == tuple(goal1[i])):
                    out1 = [start] + [goal1[i]]
                    out.append(out1)
                else:
                    paths, cost3, pathl, goal0 = a_star_3D3(self.voxmap, heuristic, tuple(start), tuple(goal1[i]), tuple(goal),
                                                       false_ceiling, traj_1, traj_2, traj_3, traj_4)
                    mylist = mylist + paths  # 路径点全部放在mylist中
                    start = paths[-1]  # 起点等于上一路径的终点
                    bend1 = bend(self.voxmap, paths)

                    # print("转弯点", bend1)
                    # print("", paths[0])
                    out1 = [paths[0]] + bend1 + [paths[-1]]
                    # print("起点+转弯点+节点", out1)

                    for i in range(len(out1)):
                        out1[i] = list(out1[i])
                    # print("out1", out1)
                    out.append(out1)
            # if np.all(tuple(start) == tuple(goal)):
            #     print("")
            # else:
            #         # 找到簇内连接后最低点，并输出
            # min_z = 999
            # min_zpo = []
            # min_zpo2 = []
            # for po in mylist:
            #     if po[2] < min_z:
            #         # min_zpo1 = po
            #         min_z = po[2]
            #         min_zpo2.append(po)
            # min_zpo.append(min_zpo2[-1])
            # print("min_z", min_z)
            # print("min_zpo", min_zpo)
            #
            # print("voxmap_goal", goal)
            # for i in range(len(min_zpo)):
            #     if goal[2] > min_zpo[i][2]:
            #         goal[2] = min_zpo[i][2]
            # print("new voxmap_goal", goal)
            paths, cost3, pathl, goal0 = a_star_3D2(self.voxmap, heuristic, start, tuple(goal), false_ceiling,
                                               traj_1, traj_2, traj_3, traj_4)
            mylist = mylist + paths
            bend2 = bend(self.voxmap, paths)
            # print("转弯点", bend2)
            # print("", paths[0])
            out2 = [paths[0]] + bend2 + [paths[-1]]
            # print("起点+转弯点+节点", out2)
            for i in range(len(out2)):
                out2[i] = list(out2[i])
            # print("out2", out2)
            out.append(out2)
            traj_goal1 = np.zeros(self.voxmap.shape, dtype=np.bool)
            traj_goal1[goal1[0][0]][goal1[0][1]][goal1[0][2]] = True
        else:
            if np.all(tuple(start) == tuple(goal)):
                # print("start,goal", start, goal)
                out3 = [start] + [goal]
                out.append(out3)
                # out.append(goal)
            else:
                mylist, cost3, pathl, goal = a_star_3D2(self.voxmap, heuristic, tuple(start), tuple(goal), false_ceiling,
                                                       traj_1, traj_2, traj_3, traj_4)
                bend1 = bend(self.voxmap, mylist)
            # print("转弯点", bend1)
            # start2 = [start]
            # goal2 = [goal]
                out3 = [mylist[0]] + bend1 + [mylist[-1]]
                for i in range(len(out3)):
                    out3[i] = list(out3[i])
                out.append(out3)
            # print("输出", out3)


        # print("goal", goal)
        traj_goal = np.zeros(self.voxmap.shape, dtype=np.bool)
        traj_goal[goal[0]][goal[1]][goal[2]] = True


        for path in mylist[1:-1]:
            traj[path] = True
        # print("路径", mylist)
        # print("输出", out)
        if len(goal1) != 0:
            World = traj | traj_start | traj_goal | traj_goal1
        else:
            World = traj | traj_start | traj_goal
        # print("traj_goal", traj_goal.shape)
        # print("world", World.shape)
        colors = np.empty(self.voxmap.shape, dtype=object)
        # color = ['orange', 'purple', 'b', 'k', 'm', 'c', 'y', 'r']
        # 给对应簇附颜色
        colors[self.voxmap] = '#000000'
        colors[traj_2] = "#96CDCD"
        colors[traj_3] = "green"
        colors[traj] = "red"
        colors[traj_goal] = "blue"
        colors[traj_start] = "y"
        if len(goal1) != 0:
            colors[traj_goal1] = "lavender"
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        # ax.set(xlim=(0, 26), ylim=(0, 50), zlim=(-10, 30))
        ax.voxels(World, facecolors=colors, edgecolor='k')
        plt.xlabel("x")
        plt.ylabel("y")
        plt.rcParams['axes.unicode_minus'] = False
        ax.set_zlabel('z')
        # plt.show()

        if len(goal1) != 0:
            World2 = traj | traj_start | traj_goal | traj_goal1
        else:
            World2 = traj | traj_start | traj_goal

        # World1 = self.voxmap | traj_3 | traj_2
        World1 = self.voxmap
        colors = np.empty(self.voxmap.shape, dtype=object)
        color = ['orange', 'lavender', 'purple', 'b', 'k', 'm', 'c', 'y', 'r']
        colors[self.voxmap] = 'darkgray'
        colors[traj_2] = "#96CDCD"
        colors[traj_3] = "green"
        colors[traj] = "red"
        colors[traj_goal] = "blue"
        colors[traj_start] = "y"
        if len(goal1) != 0:
            colors[traj_goal1] = "lavender"
        colors[self.voxmap] = 'darkgray'
        colors[traj_Nonconcrete] = "#96CDCD"
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        ax.voxels(World1, facecolors=colors, alpha=0.3, edgecolor='k')
        ax.voxels(World2, facecolors=colors, edgecolor='k')
        plt.xlabel("x")
        plt.ylabel("y")
        ax.set_zlabel('z')
        # plt.show()
        #print("out", out)

        return self.voxmap, out


def create_voxmap(data, single_obstacle, x_size, y_size, z_size):
    # data: (z, a, b, c, d, h), a,b,c,d are vertices of rectangle. h is the height.
    """
    Returns a grid representation of a 3D configuration space
    based on given obstacle data and safety distance

    The 'target_altitude': highest point + target_altitude is the height of the map.
    The 'voxel_size' argument sets the resolution of the voxel map.
    """
    # minimum and maximum coordinates
    data = np.array(data)

    # print("地图大小：", x_size, y_size, z_size)

    voxmap = np.zeros((x_size, y_size+1, z_size), dtype=np.bool)

    for i in range(data.shape[0]):
        z, a_x, a_y, b_x, b_y, c_x, c_y, d_x, d_y, h = data[i, :]

        a = [a_x, a_y]
        b = [b_x, b_y]
        c = [c_x, c_y]
        d = [d_x, d_y]
        # print(a, b, c, d)
        da_new = {"a":a, "b":b, "c":c, "d":d}
        # print("da_new.keys()", da_new.keys())

        max_value = max(a_y, b_y, c_y, d_y)
        # print("max_value", max_value)
        for key in da_new.keys():
            # print("da_new[key][1]", da_new[key][1])
            if da_new[key][1] == max_value:
                da_new[key][1] = da_new[key][1] + 2
        min_value = min(a_x, b_x, c_x, d_x)

        for key in da_new.keys():
            # print("da_new[key][1]", da_new[key][1])
            if da_new[key][0] == min_value:
                da_new[key][0] = da_new[key][0] - 1
        # print(da_new)
        # print(da_new['a'])
        # z = int(z - 10)
        # print("a, b, c, d", a, b, c, d)
        x_index, y_index = map_color_seg(a, b, c, d)
        # print("x_index, y_index",x_index, y_index)
        voxmap[x_index.astype(int), y_index.astype(int), z:(z + h)] = True
    if len(single_obstacle)>0:
        for i in range(len(single_obstacle)):
            voxmap[single_obstacle[i][0], single_obstacle[i][1], single_obstacle[i][2]] = True

    return voxmap


def map_color_seg(a, b, c, d):
    def Sort4Points(points_4):
        point_c = [(a + b + c + d) / 4 for a, b, c, d in zip(points_4[0], points_4[1], points_4[2], points_4[3])]
        dic_a_p = {}
        for point in points_4:
            angle = np.arctan2(point[1] - point_c[1], point[0] - point_c[0])
            dic_a_p[angle] = point
        return [dic_a_p[k] for k in sorted(dic_a_p.keys())]

    def linear_k_b(point1, point2):
        if point1[0] == point2[0]:
            k = 0
        else:
            k = (point1[1] - point2[1]) / (point1[0] - point2[0])
        b = point1[1] - k * point1[0]
        return k, b

    # the vertices should be in order
    a, b, c, d = Sort4Points([a, b, c, d])
    ab_linear = linear_k_b(a, b)
    bc_linear = linear_k_b(b, c)
    cd_linear = linear_k_b(c, d)
    da_linear = linear_k_b(d, a)

    # start point
    s_p_x = np.floor(np.min([a[0], b[0], c[0], d[0]]))
    s_p_y = np.floor(np.min([a[1], b[1], c[1], d[1]]))
    # end point
    e_p_x = np.ceil(np.max([a[0], b[0], c[0], d[0]]))
    e_p_y = np.ceil(np.max([a[1], b[1], c[1], d[1]]))

    offset = [s_p_x, s_p_y]
    size = [int(e_p_x - s_p_x), int(e_p_y - s_p_y)]

    ab_map = np.zeros((size[0], size[1]))
    bc_map = np.zeros((size[0], size[1]))
    cd_map = np.zeros((size[0], size[1]))
    da_map = np.zeros((size[0], size[1]))

    for x in range(size[0]):
        for y in range(size[1]):
            # two color for ab_map
            if (x + offset[0]) * ab_linear[0] + ab_linear[1] - (y + offset[1]) >= 0:
                ab_map[x][y] = 7
            else:
                ab_map[x][y] = 3
            # two color for cd_map
            if (x + offset[0]) * cd_linear[0] + cd_linear[1] - (y + offset[1]) <= 0:
                cd_map[x][y] = -7
            else:
                cd_map[x][y] = -3

            # two color for bc_map
            if (x + offset[0]) * bc_linear[0] + bc_linear[1] - (y + offset[1]) >= 0:
                bc_map[x][y] = 19
            else:
                bc_map[x][y] = 37
            # two color for da_map
            if (x + offset[0]) * da_linear[0] + da_linear[1] - (y + offset[1]) <= 0:
                da_map[x][y] = -19
            else:
                da_map[x][y] = -37
    map_all = ab_map + bc_map + cd_map + da_map
    map_all[np.nonzero(map_all)] = 1
    [x_index, y_index] = np.where(map_all == 0)
    x_index = x_index + offset[0]
    y_index = y_index + offset[1]
    return x_index.astype(int), y_index.astype(int)


class Action3D(Enum):
    """
    An action is represented by a 4 element tuple.

    The first 3 values are the delta of the action relative
    to the current voxel position. The final value
    is the cost of performing the action.
    """

    WEST = (0, -1, 0, 1)
    EAST = (0, 1, 0, 1)
    NORTH = (-1, 0, 0, 1)
    SOUTH = (1, 0, 0, 1)

    # Up & down motions
    UP = (0, 0, 1, 1)
    DOWN = (0, 0, -1, 1)

    @property
    def cost(self):
        # print("cost_value", self.value)
        return self.value[3]

    @property
    def delta(self):
        # print("delta_value", self.value)
        return (self.value[0], self.value[1], self.value[2])


def valid_actions_3D(voxel, current_node, false_ceiling=0):
    """
    Returns a list of valid actions given a voxel and current node.
    """
    valid_actions = list(Action3D)
    n, m, max_alt = voxel.shape[0] - 1, voxel.shape[1] - 1, voxel.shape[2] - 1
    x, y, z = current_node

    # check if the node is off the voxel or
    # it's an obstacle

    # 假天花为0时，表示UI输入不可以在假天花下面排布，10为假天花高度
    if z - 1 < 0 or voxel[x, y, z - 1] == 1:
        valid_actions.remove(Action3D.DOWN)
    if z + 1 > max_alt or voxel[x, y, z + 1] == 1:
        valid_actions.remove(Action3D.UP)

    if x - 1 < 0 or voxel[x - 1, y, z] == 1:
        valid_actions.remove(Action3D.NORTH)
    if x + 1 > n or voxel[x + 1, y, z] == 1:
        valid_actions.remove(Action3D.SOUTH)

    if y - 1 < 0 or voxel[x, y - 1, z] == 1:
        valid_actions.remove(Action3D.WEST)
    if y + 1 > m or voxel[x, y + 1, z] == 1:
        valid_actions.remove(Action3D.EAST)

    return valid_actions


# 生成的随机点与障碍物重合，使之避开障碍物并输出
def avoidObstalces(voxel, ran, count, false_ceiling):
    for i in range(0, count):
        for action in valid_actions_3D(voxel, ran[i], false_ceiling):
            da = action.delta
            ran[i] = (ran[i][0] + da[0], ran[i][1] + da[1], ran[i][2] + da[2])

    return ran


def heuristic(position, goal_position):
    a = abs(np.array(position)[0] - np.array(goal_position)[0])
    b = abs(np.array(position)[1] - np.array(goal_position)[1])
    c = abs(np.array(position)[2] - np.array(goal_position)[2])
    return a + b + c  # LA.norm---sqrt(x^2+y^2+……) 欧式距离公式,此处修改为曼哈顿距离公式


def a_star_3D2(voxel, h, start, goal, false_ceiling, traj_1, traj_2, traj_3, traj_4):
    path = []
    path_cost = 0
    queue = PriorityQueue()  # 优先级队列
    queue.put((0, start))  # 先进先出，数字越小优先级越高
    visited = set(start)  # start = (9,5,11), visited = {9,11,5} 去重且单独存放

    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()  # （0，start）
        current_node = item[1]  # item[1] = start
        if np.all(current_node == start):  # 做与运算，相等时输出True
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if np.all(current_node == goal):
            found = True
            break
        else:
            for action in valid_actions_3D(voxel, current_node, false_ceiling):
                # get the tuple
                cost_Nonconcrete = 1
                cost_pipe = 1
                cost_other = 1

                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])

                # 若下一节点是非结构墙的代价
                if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_Nonconcrete = 50

                # 若下一节点是非结构墙周围的代价
                if traj_3[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_pipe = 5
                if traj_4[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_other = 5

                branch_cost = current_cost + action.cost + (10 / (next_node[2]+11))  # z轴增加cost权重
                # branch_cost = current_cost + action.cost
                queue_cost = branch_cost + h(next_node, goal)

                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))

                # if current_node[2] < goal[2]:
                #     goal = [goal[0], goal[1], current_node[2]]
                #     goal = tuple(goal)
                """
                如果next_node在非结构墙中，cost+50？
                 （1）若穿非结构墙的cost很小，那么非结构墙相当于没有，可以自由穿过，聚一个类即可；
                 （2）若cost很大，就相当于结构墙，系统是不会让穿的，那么就会聚两个类。
                 那现在只需要判断next_node是否在非结构墙中，即某点是否在一个范围内
                """

    if found:

        n = goal
        # print(branch[n][0])
        path_cost = branch[n][0]
        path.append(goal)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        path_l = len(path[::-1])
    else:
        path_l = 500

    return path[::-1], path_cost, path_l, goal

def a_star_3D3(voxel, h, start, goal1, goal, false_ceiling, traj_1, traj_2, traj_3, traj_4):
    path = []
    path_cost = 0
    queue = PriorityQueue()  # 优先级队列
    queue.put((0, start))  # 先进先出，数字越小优先级越高
    visited = set(start)  # start = (9,5,11), visited = {9,11,5} 去重且单独存放

    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()  # （0，start）
        current_node = item[1]  # item[1] = start
        if np.all(current_node == start):  # 做与运算，相等时输出True
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if h(current_node, goal1) < 3:
            found = True
            goal1 = current_node
            break
        else:
            for action in valid_actions_3D(voxel, current_node, false_ceiling):
                # get the tuple
                cost_Nonconcrete = 1
                cost_pipe = 1
                cost_other = 1

                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])

                # 若下一节点是非结构墙的代价
                if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_Nonconcrete = 50

                # 若下一节点是非结构墙周围的代价
                if traj_3[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_pipe = 5
                if traj_4[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_other = 5

                branch_cost = current_cost + action.cost + (10 / (next_node[2]+1)) # z轴增加cost权重
                # branch_cost = current_cost + action.cost
                queue_cost = branch_cost + h(next_node, goal1)

                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))

                # if current_node[2] < goal[2]:
                #     goal = [goal[0], goal[1], current_node[2]]
                #     goal = tuple(goal)
                """
                如果next_node在非结构墙中，cost+50？
                 （1）若穿非结构墙的cost很小，那么非结构墙相当于没有，可以自由穿过，聚一个类即可；
                 （2）若cost很大，就相当于结构墙，系统是不会让穿的，那么就会聚两个类。
                 那现在只需要判断next_node是否在非结构墙中，即某点是否在一个范围内
                """

    if found:

        n = goal1
        # print(branch[n][0])
        path_cost = branch[n][0]
        path.append(goal1)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        path_l = len(path[::-1])
    else:
        path_l = 500

    return path[::-1], path_cost, path_l, goal1

def undiffientdata(data):
    single = 0
    for v in data:
        single = single ^ v
    return single


def adjust_bend(voxel, bend_point1, traj_2, bend, bend_point, cost3, paths, cost_new, minPo1, minPo2, ran1, voxmap_goal):
    if len(bend_point1) >= 3:
        bend_new = []
        bend_paths = bend_point1
        for i in range(len(bend_paths) - 2):
            if (bend_paths[i][2] == bend_paths[i + 1][2] and bend_paths[i + 1][2] == bend_paths[i + 2][
                2]):
                if (bend_paths[i][1] == bend_paths[i + 1][1] and bend_paths[i + 1][1] == bend_paths[i + 2][
                    1]) or (
                        bend_paths[i][0] == bend_paths[i + 1][0] and bend_paths[i + 1][0] == bend_paths[i + 2][0]):
                    print("")
                else:
                    x_arr = []
                    y_arr = []
                    x_arr.append(bend_paths[i][0])
                    x_arr.append(bend_paths[i + 1][0])
                    x_arr.append(bend_paths[i + 2][0])
                    y_arr.append(bend_paths[i][1])
                    y_arr.append(bend_paths[i + 1][1])
                    y_arr.append(bend_paths[i + 2][1])
                    # cost11是以前两个转弯的代价，cost12是现在两个转弯的代价
                    # 比较两个转弯代价的差值，超过100，表示多穿了一次非结构墙，否则可更换
                    paths1, cost12, pathl1 = a_star_3D(voxel, heuristic, tuple(bend_paths[i + 1]),
                                                       tuple(bend_paths[i + 2]), traj_2)
                    paths1, cost22, pathl1 = a_star_3D(voxel, heuristic,
                                                       (undiffientdata(x_arr), undiffientdata(y_arr),
                                                        bend_paths[i][2]),
                                                       tuple(bend_paths[i + 2]), traj_2)
                    paths2, cost11, pathl2 = a_star_3D(voxel, heuristic, tuple(bend_paths[i + 1]),
                                                       tuple(bend_paths[i]), traj_2)
                    paths2, cost21, pathl2 = a_star_3D(voxel, heuristic,
                                                       (undiffientdata(x_arr), undiffientdata(y_arr),
                                                        bend_paths[i][2]),
                                                       tuple(bend_paths[i]), traj_2)  # 新增转弯点和周围两个转折点之间的距离
                    if (cost11 + cost12) >= (cost21 + cost22):
                        if (undiffientdata(x_arr), undiffientdata(y_arr), bend_paths[i][2]) in bend_point1:
                            print("这个点", (undiffientdata(x_arr), undiffientdata(y_arr), bend_paths[i][2]))
                        else:
                            bend_paths[i + 1] = (undiffientdata(x_arr), undiffientdata(y_arr), bend_paths[i][2])
                            print("bendpaths updata:", bend_paths)
                            bend_new.append(bend_paths[i + 1])

        print("new bend paths:", bend_paths)
        if len(bend_new) == 0:
            bend = bend + len(bend_point1)
            bend_point = bend_point + bend_point1
            cost_new = cost_new + cost3
            paths_new = paths
        else:
            # 路径重新连接 paths_new
            paths_new = []
            if np.all(tuple(minPo1) == tuple(bend_paths[0])):
                print("")
            else:
                paths, cost3, pathl = a_star_3D(voxel, heuristic, tuple(minPo1), tuple(bend_paths[0]),
                                                traj_2)
                cost_new = cost_new + cost3
                paths_new = paths_new + paths[:-1]

            for i in range(len(bend_paths) - 1):
                paths, cost3, pathl = a_star_3D(voxel, heuristic, tuple(bend_paths[i]),
                                                tuple(bend_paths[i + 1]),
                                                traj_2)
                cost_new = cost_new + cost3
                paths_new = paths_new + paths[:-1]

            if np.all(tuple(minPo2) == tuple(bend_paths[-1])):
                paths_new.append(paths[-1])
            else:
                paths, cost3, pathl = a_star_3D(voxel, heuristic, tuple(bend_paths[-1]),
                                                tuple(minPo2), traj_2)
                cost_new = cost_new + cost3
                paths_new = paths_new + paths

            print("paths_new:", paths_new)
            # for path in paths_new[1:-1]:
            #     traj[path] = True

            bend_point2 = []
            if paths_new[0] in ran1:
                print("000")
            else:
                if np.all(paths_new[0] == tuple(voxmap_goal)):
                    print("")
                else:
                    bend_point2.append(paths_new[0])
            if paths_new[-1] in ran1:
                print("000")
            else:
                if np.all(paths_new[-1] == tuple(voxmap_goal)):
                    print("")
                else:
                    bend_point2.append(paths_new[-1])

            for i in range(1, len(paths_new) - 1):
                if (abs(paths_new[i - 1][0] - paths_new[i + 1][0]) == 1 and abs(
                        paths_new[i - 1][1] - paths_new[i + 1][1]) == 1) or (
                        abs(paths_new[i - 1][0] - paths_new[i + 1][0]) == 1 & abs(
                    paths_new[i - 1][2] - paths_new[i + 1][2]) == 1) or (
                        abs(paths_new[i - 1][2] - paths_new[i + 1][2]) == 1 & abs(
                    paths_new[i - 1][1] - paths_new[i + 1][1]) == 1):
                    # bend = bend + 1
                    bend_point2.append(paths_new[i])
            bend = bend + len(bend_point2)
            print("bend:", bend)
            print("bend_point2:", bend_point2)
            bend_point = bend_point + bend_point2
            print("bend_point:", len(bend_point), bend_point)
    else:
        bend = bend + len(bend_point1)
        bend_point = bend_point + bend_point1
        cost_new = cost_new + cost3
        paths_new = paths

    return paths_new, bend, bend_point, cost_new

def a_star_3D(voxel, h, start, goal, traj_2):
    path = []
    path_cost = 0
    queue = PriorityQueue()  # 优先级队列
    queue.put((0, start))  # 先进先出，数字越小优先级越高
    visited = set(start)  # start = (9,5,11), visited = {9,11,5} 去重且单独存放
    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()  # （0，start）
        current_node = item[1]  # item[1] = start
        last_node = item[1]
        if np.all(current_node == start):  # 做与运算，相等时输出True
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if np.all(current_node == goal):
            found = True
            break
        else:
            for action in valid_actions_3D(voxel, current_node):
                # get the tuple
                cost_Nonconcrete = 0
                bend_cost = 0

                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])
                # 加上非结构墙代价
                if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                    cost_Nonconcrete = 100

                branch_cost = current_cost + action.cost + (10 / (next_node[2]+1)) + cost_Nonconcrete  # z轴增加cost权重
                queue_cost = branch_cost + h(next_node, goal)

                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))
                    last_node = current_node

                """
                如果next_node在非结构墙中，cost+50？
                 （1）若穿非结构墙的cost很小，那么非结构墙相当于没有，可以自由穿过，聚一个类即可；
                 （2）若cost很大，就相当于结构墙，系统是不会让穿的，那么就会聚两个类。
                 那现在只需要判断next_node是否在非结构墙中，即某点是否在一个范围内
                """
    if found:
        n = goal
        path_cost = branch[n][0]
        path.append(goal)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        path_l = len(path[::-1])
    else:
        path_l = 500

    return path[::-1], path_cost, path_l

def bend(voxel, paths):
    bend = []
    for i in range(1, len(paths) - 1):
        if (abs(paths[i - 1][0] - paths[i + 1][0]) == 1 and abs(
                paths[i - 1][1] - paths[i + 1][1]) == 1) or (
                abs(paths[i - 1][0] - paths[i + 1][0]) == 1 & abs(
            paths[i - 1][2] - paths[i + 1][2]) == 1) or (
                abs(paths[i - 1][2] - paths[i + 1][2]) == 1 & abs(
            paths[i - 1][1] - paths[i + 1][1]) == 1):
            # bend = bend + 1
            bend.append(paths[i])
    return bend